package com.frinder.astar;

import com.google.common.collect.Lists;
import com.google.common.collect.Maps;
import com.google.common.collect.Queues;
import com.google.common.collect.Sets;
import org.slf4j.Logger;
import org.slf4j.LoggerFactory;

import java.util.*;
import java.util.concurrent.Callable;

import static com.frinder.astar.Constants.BAR;
import static com.frinder.astar.Constants.DIRECT_VALUE;

/**
 * Created by kiktech on 2018/2/10.
 */
public class PathFinder implements Callable<PathNodeList> {

    private final Logger logger = LoggerFactory.getLogger(getClass());

    /**
     * map nodes
     */
    private final Map<String, Node> mapNodes = Maps.newConcurrentMap();

    /**
     * block node
     */
    private final Set<String> blockedNodes = Sets.newHashSet();

    /**
     * 优先队列（升序）
     */
    private final Queue<Node> openList = Queues.newPriorityQueue();

    /**
     * 封闭列表
     */
    private final List<Node> closeList = Lists.newArrayList();

    /**
     * 障碍估值比率
     */
    private double currBlockedRate;

    private PathNodeList pathNodeList;

    private AStar aStar;

    public PathFinder(AStar aStar, double currBlockedRate) {
        this.aStar = aStar;
        this.currBlockedRate = currBlockedRate;
        int[][] map = aStar.getMap();
        for (int y = 0; y < aStar.getHeight(); y++) {
            for (int x = 0; x < aStar.getWidth(); x++) {
                boolean blocked = map[y][x] == BAR;
                Node node = new Node.Builder()
                        .point(new Point.Builder().x(x).y(y).build())
                        .blocked(blocked)
                        .build();
                mapNodes.put(node.getPoint().getName(), node);
                if (blocked) {
                    blockedNodes.add(node.getPoint().getName());
                }
            }
        }
    }

    @Override
    public PathNodeList call() throws Exception {
        findPath();
        return pathNodeList;
    }

    /**
     * 查找路径方法
     */
    public void findPath() {
        openList.clear();
        closeList.clear();
        openList.add(aStar.getSourceNode());
        moveNodes();
    }

    /**
     * 移动节点
     */
    private void moveNodes() {
        long count = 0;
        while (!openList.isEmpty()) {
            if (isCloseNode(aStar.getDestNode().getPoint())) {
                // 清空 open / close 列表
                openList.clear();
                closeList.clear();
                drawPath();
                break;
            }
            Node currNode = openList.poll();
            closeList.add(currNode);
            addNeighborNodesToOpenList(currNode);
            ++count;
            if (count > 5000) {
                logger.error("规划路径失败！");
                break;
            }
        }
    }

    /**
     * 从 dest node 依次向上查找 parent node
     * <p>
     * 内存中保存一份链表
     */
    private void drawPath() {
        List<Point> pointList = Lists.newArrayList();
        Node destNode = aStar.getDestNode();
        while (null != destNode) {
            Point point = destNode.getPoint();
            pointList.add(point);
            destNode = destNode.getParentNode();
            // 如果某节点的父节点为目标点，则终止查找，防止可能出现的死循环
            // 2个节点互为子父节点，导致死循环发生
            if (aStar.getDestNode().equals(destNode)) {
                break;
            }
            // 如果某节点为起点，则终止查找，防止出现多条 代价 相同的路径
            if (destNode.equals(aStar.getSourceNode())) {
                break;
            }
        }
        pathNodeList = new PathNodeList.Builder()
                .name(String.valueOf(currBlockedRate))
                .points(pointList)
                .build();
    }

    /**
     * 添加当前 node 相邻 node 到 open list
     *
     * @param currNode
     */
    private void addNeighborNodesToOpenList(Node currNode) {
        int x = currNode.getPoint().getX();
        int y = currNode.getPoint().getY();
        getDirectNeighborNodes(x, y).stream()
                .filter(n -> checkAddNode(n.getX(), n.getY()))
                .forEach(n -> addNeighborNodeToOpenList(currNode, n));
//        logger.info("openList: {}", openList);
//        logger.info("closeList: {}", closeList);
//        getObliqueNeighborNodes(x, y).stream()
//                .filter(n -> checkAddNode(n.getX(), n.getY()))
//                .forEach(n -> addNeighborNodeToOpenList(currNode, n, Constants.OBLIQUE_VALUE));
    }

    /**
     * 添加单个 node 到 open list
     *
     * @param currNode
     * @param point
     */
    private void addNeighborNodeToOpenList(Node currNode, Point point) {
        Node destNode = aStar.getDestNode();
        double g = destNode.getG() + getV(point);
        Optional<Node> childNode = findNodeInOpenList(point);
        Node node;
        if (childNode.isPresent()) {
            node = childNode.get();
            node.setG(g);
            node.setParentNode(currNode);
        } else {
            double h = currNode.getH() + getH(destNode.getPoint(), point);
            if (isDestNode(destNode.getPoint(), point)) {
                node = destNode;
                node.setG(g);
                node.setH(h);
                node.setParentNode(currNode);
            } else {
                node = new Node.Builder()
                        .point(point)
                        .parentNode(currNode)
                        .g(g)
                        .h(h)
                        .build();
            }
        }
        // 重新调整堆
        openList.add(node);
    }

    /**
     * 是否 dest node
     *
     * @param destNode
     * @param sourceNode
     * @return
     */
    private boolean isDestNode(Point destNode, Point sourceNode) {
        return null != sourceNode && destNode.equals(sourceNode);
    }

    /**
     * 是否已存在 close list
     *
     * @param x
     * @param y
     * @return
     */
    private boolean isCloseNode(int x, int y) {
        final Point point = new Point.Builder().x(x).y(y).build();
        return isCloseNode(point);
    }

    /**
     * 是否已存在 close list
     *
     * @param point
     * @return
     */
    private boolean isCloseNode(Point point) {
        return closeList.stream().anyMatch(c -> point.equals(c.getPoint()));
    }

    /**
     * 判断是否已经存在于 open list
     *
     * @param x
     * @param y
     * @return
     */
    private boolean isOpenNode(int x, int y) {
        final Point point = new Point.Builder().x(x).y(y).build();
        return isOpenNode(point);
    }

    /**
     * 判断是否已经存在于 open list
     *
     * @param point
     * @return
     */
    private boolean isOpenNode(Point point) {
        return openList.stream().anyMatch(o -> point.equals(o.getPoint()));
    }

    /**
     * 判断是否可以添加进 open list
     *
     * @param x
     * @param y
     * @return
     */
    private boolean checkAddNode(int x, int y) {
        // 判断是否在地图中
        if (x < 0 || y < 0) {
            return false;
        }
        if (x >= aStar.getWidth() || y >= aStar.getHeight()) {
            return false;
        }
        // 判断是否为不可访问点
        if (BAR == aStar.getMap()[y][x]) {
            return false;
        }
        // 判断是否在开放列表
        if (isOpenNode(x, y)) {
            return false;
        }
        // 判断是否已存在于封闭列表
        if (isCloseNode(x, y)) {
            return false;
        }
        return true;
    }

    /**
     * 获取估算值 H，估算值为不考虑障碍时理想代价
     *
     * @param destNode
     * @param currNode
     * @return
     */
    private int getH(Point destNode, Point currNode) {
        return Math.abs(destNode.getX() - currNode.getX()) + Math.abs(destNode.getY() - currNode.getY());
    }

    /**
     * 从 open list 中获取 node
     *
     * @param x
     * @param y
     * @return
     */
    private Optional<Node> findNodeInOpenList(int x, int y) {
        Point point = new Point.Builder().x(x).y(y).build();
        return findNodeInOpenList(point);
    }

    /**
     * 从 open list 中获取 node
     *
     * @param point
     * @return
     */
    private Optional<Node> findNodeInOpenList(Point point) {
        return openList.stream().filter(o -> point.equals(o.getPoint())).findFirst();
    }

    /**
     * 获取 v
     *
     * @param point
     * @return
     */
    private double getV(Point point) {
        long count = getDirectNeighborNodes(point.getX(), point.getY())
                .stream()
                .filter(o -> blockedNodes.contains(o.getName()))
                .count();
        return DIRECT_VALUE + count * currBlockedRate;
    }

    /**
     * @param x
     * @param y
     * @return
     */
    private List<Point> getDirectNeighborNodes(int x, int y) {
        // 顺时针依次添加
        return Lists.newArrayList(
                // 左、上、右、下
                new Point.Builder().x(x - 1).y(y).build(),
                new Point.Builder().x(x).y(y - 1).build(),
                new Point.Builder().x(x + 1).y(y).build(),
                new Point.Builder().x(x).y(y + 1).build()
        );
    }

    /**
     * @param x
     * @param y
     * @return
     */
    private List<Point> getObliqueNeighborNodes(int x, int y) {
        // 顺时针依次添加
        return Lists.newArrayList(
                // 左上、右上、右下、左下
                new Point.Builder().x(x - 1).y(y - 1).build(),
                new Point.Builder().x(x + 1).y(y - 1).build(),
                new Point.Builder().x(x + 1).y(y + 1).build(),
                new Point.Builder().x(x - 1).y(y + 1).build()
        );
    }

}
